#include "qe_log.h"
#include "qe_string.h"
#include "qe_memory.h"
#include "qe_assert.h"
#include "qe_driver.h"
#include "libfdt.h"



QELOG_DOMAIN("ps7-gpio");

#define PS7_GPIO_NUM_BANKS          (4)
#define PS7_GPIO_NUM_PINS           (118)

#define PS7_GPIO_DATA_LSW_OFFSET    0x00000000U  /* Mask and Data Register LSW, WO */
#define PS7_GPIO_DATA_MSW_OFFSET    0x00000004U  /* Mask and Data Register MSW, WO */

#define PS7_GPIO_DATA_MASK_OFFSET   0x00000008U  /* Data/Mask Registers offset */
#define PS7_GPIO_REG_MASK_OFFSET    0x00000040U

#define PS7_GPIO_DIRM	            0x00000204U  /* Direction Mode Register, RW */
#define PS7_GPIO_OUTEN	            0x00000208U  /* Output Enable Register, RW */
#define PS7_GPIO_INT_DIS            0x00000214U


typedef struct 
{
    volatile qe_u32 lsw;            /* Maskable Output Data Lower 16bits */
    volatile qe_u32 msw;            /* Maskable Output Data Upper 16bits */
} bank_mask_reg;

typedef struct
{
    volatile qe_u32 data;           /* Output Data */
} bank_output_reg;

typedef struct
{
    volatile qe_u32 data;           /* Input Data */
} bank_input_reg;

typedef struct
{
    volatile qe_u32 dirm;           /* Direction Mode Register */
    volatile qe_u32 outen;          /* Output enable */
    volatile qe_u32 intmask;        /* Interrupt Mask Status */
    volatile qe_u32 inten;          /* Interrupt Enable */
    volatile qe_u32 intdis;         /* Interrupt Disable */
    volatile qe_u32 intsts;         /* Interrupt Status */
    volatile qe_u32 inttype;        /* Interrupt Type */
    volatile qe_u32 intpol;         /* Interrupt Polarity */
    volatile qe_u32 intany;         /* Interrupt Any Edge Sensitive */
} bank_mode_reg;

typedef struct 
{
    bank_mask_reg masks[PS7_GPIO_NUM_BANKS];
    bank_output_reg outputs[PS7_GPIO_NUM_BANKS];
    bank_input_reg inputs[PS7_GPIO_NUM_BANKS];
    bank_mode_reg modes[PS7_GPIO_NUM_BANKS];
} ps7_gpio_reg;

typedef struct
{
    qe_gpio_dev gpio;
    ps7_gpio_reg *reg;
    qe_uint num_pins;
} ps7_gpio_dev;

static void ps7_gpio_hw_init(ps7_gpio_dev *dev)
{
    int i;
    qe_u32 offset;

    /* disable interrupts for all pins in all banks */
    for (i=0; i<PS7_GPIO_NUM_BANKS; i++) {
        dev->reg->modes[i].intdis = 0xFFFFFFFFU;
    }
}

static void get_bank(qe_u8 pin, qe_u8 *bank, qe_u8 *pin_num)
{
    qe_u32 tab[6] = {0};

    tab[0] = 31;    /* 0 - 31, Bank 0 */
    tab[1] = 53;    /* 32 - 53, Bank 1 */
    tab[2] = 85;    /* 54 - 85, Bank 2 */
    tab[3] = 117;   /* 86 - 117 Bank 3 */

    *bank = 0U;

    while (*bank < 4U) {
        if (pin <= tab[*bank]) {
            break;
        }
        (*bank)++;
    }

	if (*bank == (qe_u8)0) {
		*pin_num = pin;
	} else {
		*pin_num = (qe_u8)((qe_u32)pin %
					(tab[*bank - (qe_u8)1] + (qe_u32)1));
	}
}

static void set_direction(ps7_gpio_dev *dev, qe_uint pin, qe_u8 direction)
{
    qe_u8 bank;
    qe_u8 pin_num;

    get_bank(pin, &bank, &pin_num);

	if (direction != 0) { /*  Output Direction */
        dev->reg->modes[bank].dirm |= ((qe_u32)1 << (qe_u32)pin_num);
	} else { /* Input Direction */
		dev->reg->modes[bank].dirm &= ~ ((qe_u32)1 << (qe_u32)pin_num);
	}
}

static void output_enable(ps7_gpio_dev *dev, qe_uint pin, qe_bool en)
{
    qe_u8 bank;
    qe_u8 pin_num;

    get_bank(pin, &bank, &pin_num);

	if (en != 0) { /*  Enable Output Enable */
        dev->reg->modes[bank].outen |= ((qe_u32)1 << (qe_u32)pin_num);
	} else { /* Disable Output Enable */
		dev->reg->modes[bank].outen &= ~ ((qe_u32)1 << (qe_u32)pin_num);
	}
}

static qe_ret set_value(ps7_gpio_dev *dev, qe_u8 pin, qe_u8 val)
{
	qe_u32 value;
	qe_u8 bank;
	qe_u8 pin_num;
	qe_u32 data_val = val;

	/* Get the Bank number and Pin number within the bank. */
	get_bank(pin, &bank, &pin_num);

	if (pin_num > 15U) {
		/* There are only 16 data bits in bit maskable register. */
		pin_num -= (qe_u8)16;
	}

	/*
	 * Get the 32 bit value to be written to the Mask/Data register where
	 * the upper 16 bits is the mask and lower 16 bits is the data.
	 */
	data_val &= (qe_u32)0x01;
	value = ~((qe_u32)1 << (pin_num + 16U)) & ((data_val << pin_num) | 0xFFFF0000U);

    if (pin_num > 15U) {
        dev->reg->masks[bank].msw = value;
    } else {
        dev->reg->masks[bank].lsw = value;
    }

    return qe_ok;
}

static qe_ret ps7_gpio_direction_input(qe_gpio_dev *gpio, qe_uint pin)
{
    ps7_gpio_dev *dev = (ps7_gpio_dev *)gpio->parent.priv;

    if (pin >= PS7_GPIO_NUM_PINS) {
        qe_error("%s pin:%d out of range", gpio->parent.name, pin);
        return qe_err_range;
    }

    output_enable(dev, pin, 0);
    set_direction(dev, pin, 0);

    qe_debug("%s pin:%d set input", gpio->parent.name, pin);

    return qe_ok;
}

static qe_ret ps7_gpio_direction_output(qe_gpio_dev *gpio, qe_uint pin, qe_int value)
{
    ps7_gpio_dev *dev = (ps7_gpio_dev *)gpio->parent.priv;

    if (pin >= PS7_GPIO_NUM_PINS) {
        qe_error("%s pin:%d out of range", gpio->parent.name, pin);
        return qe_err_range;
    }

    set_direction(dev, pin, 1);
    output_enable(dev, pin, 1);
    set_value(dev, pin, value);

    qe_debug("%s pin:%d set output", gpio->parent.name, pin);

    return qe_ok;
}

static qe_int ps7_gpio_get_value(qe_gpio_dev *gpio, qe_uint pin)
{
    qe_u8 bank;
    qe_u8 pin_num;

    ps7_gpio_dev *dev = (ps7_gpio_dev *)gpio->parent.priv;

    get_bank(pin, &bank, &pin_num);

    return dev->reg->inputs[bank].data & (1 << pin_num);
}

static qe_ret ps7_gpio_set_value(qe_gpio_dev *gpio, qe_uint pin, qe_int value)
{
    qe_u8 bank;
    qe_u8 pin_num;

    ps7_gpio_dev *dev = (ps7_gpio_dev *)gpio->parent.priv;

    set_value(dev, pin, value);

    return qe_ok;
}

static void ps7_gpio_show(qe_gpio_dev *dev)
{
    ps7_gpio_dev *gpio = (ps7_gpio_dev *)dev;

    qe_printf("name      : %s\r\n", dev->parent.name);
    qe_printf("regs      : %x\r\n", gpio->reg);
    qe_printf("num pins  : %x\r\n", PS7_GPIO_NUM_PINS);
    qe_printf("num banks : %x\r\n", PS7_GPIO_NUM_BANKS);
}

static void ps7_gpio_list_pins(qe_gpio_dev *dev)
{
    int pin;
    qe_u8 bank;
    qe_u8 pin_num;
    ps7_gpio_dev *gpio = (ps7_gpio_dev *)dev;

    qe_const_str head_fmt = "%-5s%-12s%-5s\r\n";
    qe_const_str line_fmt = "%-5d%-12s%-5d\r\n";

    qe_printf(head_fmt, "pin", "direction", "value");

    for (pin=0; pin<PS7_GPIO_NUM_PINS; pin++) {
        
        get_bank(pin, &bank, &pin_num);
        
        if (gpio->reg->modes[bank].dirm & (1 << pin_num)) {
            qe_printf(line_fmt, pin, "in", gpio->reg->outputs[bank].data & (1 << pin_num));
        } else {
            qe_printf(line_fmt, pin, "out", gpio->reg->inputs[bank].data & (1 << pin_num));
        }
    }
}

static qe_gpio_ops ps7_gpio_ops = {
    .direction_input = ps7_gpio_direction_input,
    .direction_output = ps7_gpio_direction_output,
    .get_value = ps7_gpio_get_value,
    .set_value = ps7_gpio_set_value,
    .show = ps7_gpio_show,
    .dump_regs = QE_NULL,
    .list_pins = ps7_gpio_list_pins,
};

static qe_ret ps7_gpio_probe(const void *fdt, int offset)
{
	int len;
    const char *name;
    const qe_u32 *prop_reg;
    ps7_gpio_dev *dev;

    name = fdt_get_name(fdt, offset, QE_NULL);

    prop_reg = (const qe_u32 *)fdt_getprop(fdt, offset, "reg", &len);
    if (!prop_reg) {
        qe_error("%s no reg prop", name);
        return qe_err_param;
    }

    dev = qe_malloc(sizeof(ps7_gpio_dev));
    qe_assert(dev);

    dev->reg = (ps7_gpio_reg *)(qe_ubase)fdt32_to_cpu(*prop_reg);

    qe_gpio_register(&dev->gpio, name, &ps7_gpio_ops, dev);

    qe_debug("register %s", name);

    return qe_ok;
}

static const qe_device_id ps7_gpio_ids[] = {
    {.compatible = "xlnx,zynq-gpio",},
    {}
};

QE_DRIVER(ps7_gpio) = {
    .name = "ps7-gpio",
    .of_match = ps7_gpio_ids,
    .probe = ps7_gpio_probe,
};

QE_DRIVER_FORCE_EXPORT(ps7_gpio);